//
// Created by engineai on 2024/07/03.
//

#ifndef ZQ_HUMANOID_CONTACTIMPULSE_H
#define ZQ_HUMANOID_CONTACTIMPULSE_H

#include "ContactConstraint.h"
#include "../Dynamics/FloatingBaseModel.h"

/*!
 * Impulse-based contact dynamics for a FloatingBaseModel
 */
template <typename T>
class ContactImpulse : public ContactConstraint<T>
{
public:
    /*!
     * Construct a new contact dynamics model for a given FloatingBaseModel
     * @param model : The floating base model used for contacts
     */
    ContactImpulse(FloatingBaseModel<T> *model) : ContactConstraint<T>(model)
    {
        _iter_lim = 10;
        _nDof = CC::_model->_nDof;
        _b_debug = false;
    }

    virtual ~ContactImpulse() {}

    /*!
     * Impulse-based contact dynamics do not adjust the external forces, they modify q_dot
     * instead.
     *
     * Used for spring damper based contact constraint method,
     * therefore almost empty function in this class.
     * Only used to update dt and penetration recovery ratio
     * @param dt : time step (sec)
     */
    virtual void UpdateExternalForces(T K, T D, T dt)
    {
        (void)K;
        (void)D;
        // Set penetration recovery ration here
        _penetration_recover_ratio = 0.0 / dt;
        _dt = dt;
    }

    virtual void UpdateQdot(FBModelState<T> &state);

protected:
    size_t _iter_lim;
    bool _b_debug;
    T _tol = 1.e-6;
    T _penetration_recover_ratio;
    size_t _nDof;
    void _UpdateVelocity(DVec<T> &qdot);
    void _UpdateQdotOneDirection(size_t idx,
                                 const vectorAligned<D3Mat<T>> &Jc_list,
                                 const T *lambda_list,
                                 const vectorAligned<DVec<T>> AinvB_list,
                                 const T *des_vel_list, const T *min_list,
                                 const T *max_list, DVec<T> &qdot);
    size_t _iter_sum;

private:
    T _dt;
};
#endif // ZQ_HUMANOID_CONTACTIMPULSE_H
